#ifndef PATH_EVALUATOR_H
#define PATH_EVALUATOR_H

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include "ompl_includes.h"
#include "StatePtr.h"
#include "FrictionModel.h"

class PathEvaluator {

  ompl::RNG rng;
  const boost::shared_ptr<FrictionModel> frictionModel;
  
  double estimateProbability(double x, double y) {
    double acc = 0.0;
    const int N = 13; // number of samples
    for (int i = 0; i < N; ++i) {
      acc += frictionModel->probability(x + rng.uniformReal( -0.4, 0.4 ), y + rng.uniformReal( -0.4, 0.4 ));
    }
    return acc / static_cast<double>(N);
  }

public:
  PathEvaluator(const boost::shared_ptr<FrictionModel> frictionModel)
  : frictionModel(frictionModel)
  {
  }

  double motionProbability(const ompl::geometric::PathGeometric & path) {
    std::size_t stateCount = path.getStateCount();
    double p = 1.0;
    for (std::size_t i = 1; i < stateCount; ++i) {
      const ompl::base::State * state = path.getState(i);
      typedef ompl::base::SE2StateSpace::StateType SE2StateType;
      const SE2StateType * se2state = dynamic_cast<const SE2StateType*>(state);
      p *= estimateProbability(se2state->getX(), se2state->getY());
    }
    return p;
  }
};

#endif
